#include "Game Engine/Header/Physics.h"
#include "Game Engine/Header/Camera.h"

Physics physics;

extern ofstream g_debug;

//////////////////////////////////////////////////////////////
//						PhysicsEngine						//
//////////////////////////////////////////////////////////////

Physics::Physics()
{

}

Physics::~Physics()
{
	Release();
}

void Physics::Init( btDynamicsWorld* world, D3DXVECTOR3 pos, D3DXVECTOR3 size )
{
	mDynamicsWorld = world;
	//srand(GetTickCount());

	////New default Collision configuration
	//btDefaultCollisionConfiguration *cc;
	//cc = new btDefaultCollisionConfiguration();

	////New default Constraint solver
	//btConstraintSolver *sl;
	//sl = new btSequentialImpulseConstraintSolver();

	////New axis sweep broadphase
	//btVector3 worldAabbMin(-1000,-1000,-1000);
	//btVector3 worldAabbMax(1000,1000,1000);
	//const int maxProxies = 32766;
	//btBroadphaseInterface *bp;
	//bp = new btAxisSweep3(worldAabbMin, worldAabbMax, maxProxies);

	////new dispatcher
	//btCollisionDispatcher *dp;
	//dp = new btCollisionDispatcher(cc);

	////finally create the dynamics world
	//mDynamicsWorld = new btDiscreteDynamicsWorld(dp, bp, sl, cc);

	//Create "floor" plane
	m_floor = new btRigidBody(0.0f, new btDefaultMotionState(), new btStaticPlaneShape(btVector3(0,1,0),-0.5));
	mDynamicsWorld->addRigidBody(m_floor);

	//Reset();

	//for( UINT i = 0; i < numObjects; i ++ )
	//{
	//	env[i] = new Player( mesh );
	//	mDynamicsWorld->addRigidBody(env[i]->mBody);
	//}

	for( UINT i = 0; i < 1; i ++ )
		m_boxes.push_back( CreateOBB( pos,
								D3DXVECTOR3( 1.0f, 1.0f, 1.0f ) ) );
}

void Physics::Release()
{
	if(mDynamicsWorld != NULL)
		delete mDynamicsWorld;
}

void Physics::Reset()
{
	//Remove Old boxes
	for(int i=0; i<(int)m_boxes.size(); i++)
	{
		mDynamicsWorld->removeRigidBody(m_boxes[i]->mBody);
		delete m_boxes[i];
	}	
	m_boxes.clear();

	//Create new ones...

}

void Physics::Update( float deltaTime )
{
	if(mDynamicsWorld != NULL)
	{
		mDynamicsWorld->stepSimulation(deltaTime);
	}

	//btVector3 linear = m_boxes[0]->mBody->getLinearFactor();
	//float x = linear.x();
	//float y = linear.y();
	//float z = linear.z();
	//
	//
	//float xVel = env.Velocity().x*800;
	//float yVel = env.Velocity().y*800;
	//float zVel = env.Velocity().z*800;

	//if( xVel < 0.005f && xVel > -0.005f )
	//	xVel = 0.0f;
	//if( yVel < 0.005f && yVel > -0.005f )
	//	yVel = 0.0f;
	//if( zVel < 0.005f && zVel > -0.005f )
	//	zVel = 0.0f;

	//btVector3 vel = btVector3( xVel, 0.0f, zVel );
	////m_boxes[0]->mBody->setLinearFactor(btVector3(1,1,1));
	////body->setRestitution(1);

	////btVector3 linVel(vel);
	////linVel.normalize();
	////linVel*=m_ShootBoxInitialSpeed;

	//m_boxes[0]->mBody->setLinearVelocity( vel );
	//m_boxes[0]->mBody->getWorldTransform().setRotation(btQuaternion(0,0,0,1));
	//m_boxes[0]->mBody->setLinearVelocity(linVel);
	//m_boxes[0]->mBody->setAngularVelocity(btVector3(0,0,0));
	//m_boxes[0]->mBody->setContactProcessingThreshold(1e30);
}

void Physics::Render( Mesh& mesh, D3DXMATRIX& lightViewProj, UINT count )
{
	//for(int i=0; i<(int)m_boxes.size(); i++)
	//{
		m_boxes[count]->Render( mesh, lightViewProj );
	//}
}

OBB* Physics::CreateOBB( D3DXVECTOR3 pos, D3DXVECTOR3 size )
{
	OBB *obb = new OBB( pos, size );
	mDynamicsWorld->addRigidBody(obb->mBody);
	return obb;
}

D3DXVECTOR3 Physics::RandomVector(D3DXVECTOR3 &min, D3DXVECTOR3 &max)
{
	return D3DXVECTOR3(min.x + (max.x - min.x) * RandomFloat(),
					   min.y + (max.y - min.y) * RandomFloat(),
					   min.z + (max.z - min.z) * RandomFloat());
}

float Physics::RandomFloat()
{
	return (rand()%1000) / 1000.0f;
}